#include "include/config_qn.h"
#include "include/qn_pdo.h"

Robot_Aix A1,A2,A3,A4;

int config_PDO(){
    
    int flag=0;
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    else check_master_state();
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    if(!sc_cooldrive0){
    	fprintf(stderr, "Failed to get slave0 config.\n");
        return -1;
    }
    else flag=ecrt_slave_config_pdos(sc_cooldrive0, EC_END, slave_0_syncs);
    
    if (!flag) {
        fprintf(stderr, "Failed to configure slave0 PDOs.\n");
        return -1;
    }else fprintf(stderr, "success to configure slave0 PDOs.\n");
    
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive); 
    if(!sc_cooldrive0){
    	fprintf(stderr, "Failed to get slave0 config.\n");
        return -1;
    }
    else flag=ecrt_slave_config_pdos(sc_cooldrive1, EC_END, slave_1_syncs);
    
    if (!flag) {
        fprintf(stderr, "Failed to configure slave1 PDOs.\n");
        return -1;
    }else fprintf(stderr, "success to configure slave1 PDOs.\n");
    
   return 0;
}

int config_cooldrive(){
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();
    
    printf("step2:master get slave config\n");
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);       
    //check_slave_config_states();
    printf("step2.1:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master);     
    sleep(3);
    printf("step2.2:check salve config states.\n");    
    check_slave0_config_states();
    check_slave1_config_states();
    //pause();
    printf("step3:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    domainServoInput = ecrt_master_create_domain(master);
    printf("step3.1:domain reg pdo entry.\n");
    
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6040, 0x00,domainServoInput,&A1.cntlwd);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6041, 0x00,domainServoOutput,&A1.status);
    printf("step3.2:check domain.\n");
    check_domainInput_state();
    check_domainOutput_state();
    
    /*
    printf("step4:configure SYNC signals.\n");
    ecrt_master_select_reference_clock(master,sc_cooldrive0);
    // configure SYNC signals for this slave
    //ecrt_slave_config_dc(sc_cooldrive0, 0x0300, 1000000, 0, 0, 0);
    //ecrt_slave_config_dc(sc_cooldrive1, 0x0300, 1000000, 0, 0, 0);
    
    printf("enable aix4.\n");
    sleep(3);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6840,0x00,16);
    
    ecrt_slave_config_sdo16(sc_cooldrive0,0x1c32,0x01,0x02);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x1c32,0x01,0x02);
    sleep(1);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x6040,0x00,15);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6040,0x00,15);
    sleep(1);
    ecrt_slave_config_sdo32(sc_cooldrive0,0x21b0,0x0100,0);
    ecrt_slave_config_sdo32(sc_cooldrive1,0x21b0,0x0100,0);
    */
   
    
    
    

}

int config_qn_noreset(){
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();
    
    printf("step2:master get slave config\n");
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);  

    sleep(3);
    printf("step2.2:check salve config states.\n");    
    check_slave0_config_states();
    check_slave1_config_states();
    
    printf("step2.1:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master);     
    sleep(3);
    printf("step2.2:check salve config states.\n");    
    check_slave0_config_states();
    check_slave1_config_states();
}


int config_pp_mode(){
    master = ecrt_request_master(0);
    if (!master) {
        fprintf(stderr, "Failed to request master.\n");
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();
    
    
    printf("step2:master get slave config\n");
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);  
    
    ecrt_slave_config_sdo8(sc_cooldrive0,0x1600,0x00,0x02);
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    ecrt_slave_config_sdo8(sc_cooldrive0,0x6060,0x00,0x01);
    ecrt_slave_config_sdo8(sc_cooldrive0,0x6860,0x00,0x01);
    ecrt_slave_config_sdo8(sc_cooldrive1,0x6060,0x00,0x01);
    ecrt_slave_config_sdo8(sc_cooldrive1,0x6860,0x00,0x01);
    
    ecrt_slave_config_sdo16(sc_cooldrive0,0x1c32,0x01,0x00);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x1c32,0x01,0x00);
    
    //ecrt_slave_config_sdo16(sc_cooldrive0,0x1c32,0x02,4000);
    //ecrt_slave_config_sdo16(sc_cooldrive1,0x1c32,0x02,4000);
    
    printf("step2.1:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master);     
    
    sleep(3);
    printf("step2.2:check salve config states.\n");    
    check_slave0_config_states();
    check_slave1_config_states();
    /**
    printf("clear fault\n");
    sleep(1);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x6040,0x00,0x02);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x6840,0x00,0x02);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6040,0x00,0x02);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6840,0x00,0x02);
    sleep(1);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x6040,0x00,0x0f);
    ecrt_slave_config_sdo16(sc_cooldrive0,0x6840,0x00,0x0f);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6040,0x00,0x0f);
    ecrt_slave_config_sdo16(sc_cooldrive1,0x6840,0x00,0x0f);
    ***/
    printf("step3:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    domainServoInput = ecrt_master_create_domain(master);
    printf("step3.1:domain reg pdo entry.\n");
    
    qn_pdo_reg(sc_cooldrive0);
    printf("drive0 reg\n");
    qn_pdo_reg(sc_cooldrive1);
    printf("drive1 reg\n");
    //ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive0,2,1,0,domainServoInput,NULL);
    //ecrt_slave_config_reg_pdo_entry_pos(sc_cooldrive0,3,1,0,domainServoOutput,NULL);
    /**
    int ret=1;
    ret=ecrt_domain_reg_pdo_entry_list(domainServoOutput,domainServoOutput_regs);
    if(ret){ printf("domain_out reg suscess\n");}
    ret=ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs);
    if(ret){ printf("domain_in reg suscess\n");}
    /*
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6040, 0x00,domainServoInput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6041, 0x00,domainServoOutput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6840, 0x00,domainServoInput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6841, 0x00,domainServoOutput,NULL);
    
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive1,0x6040, 0x00,domainServoInput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive1,0x6041, 0x00,domainServoOutput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive1,0x6840, 0x00,domainServoInput,NULL);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive1,0x6841, 0x00,domainServoOutput,NULL);
    */
    printf("step3.2:check domain.\n");
    check_domainInput_state();
    check_domainOutput_state();



}



int qn_pdo_reg(ec_slave_config_t *sc){
    
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,0,0,domainServoOutput,NULL);   
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,1,0,domainServoOutput,NULL);  
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,0,1,domainServoOutput,NULL);   
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,1,1,domainServoOutput,NULL);  
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,0,2,domainServoOutput,NULL);   
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,1,2,domainServoOutput,NULL); 
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,0,4,domainServoOutput,NULL);   
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,1,4,domainServoOutput,NULL); 
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,0,5,domainServoOutput,NULL);   
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,1,5,domainServoOutput,NULL); 
    	
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,0,0,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,1,0,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,2,0,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,3,0,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,0,1,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,1,1,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,2,1,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,3,1,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,0,2,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,1,2,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,2,2,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,3,2,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,0,3,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,1,3,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,2,3,domainServoInput,NULL);
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,3,3,domainServoInput,NULL);
    	
    	
}


int qn_pdo_reg1(ec_slave_config_t *sc){
    int i;
    for(i=0;i<12;i++){
        //if(i==3) break;
        //if(i==8) break;
    	ecrt_slave_config_reg_pdo_entry_pos(sc,2,i+1,0,domainServoInput,NULL);   
    	printf("input:%d\n",i+1); 	
    }
    for(i=0;i<18;i++){
    	//if(i==3) break;
        //if(i==8) break;
    	ecrt_slave_config_reg_pdo_entry_pos(sc,3,i+1,0,domainServoOutput,NULL);
    	printf("output:%d\n",i+1); 
    	
    }
}

int remap_qn_pdo(ec_slave_config_t *sc){


  //Set Rx PDO Entries object
  ecrt_slave_config_sdo8(sc, 0x1C12, 0, 0);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1600);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1610);
  // Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1C12, 0, 2);

  //------------------------------- 0x1600 -------------------------------
  //Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1600, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1600, 1, 0x60400010);// '-- Control Word
  ecrt_slave_config_sdo32(sc, 0x1600, 2, 0x607A0020);//'-- Target Position
  ecrt_slave_config_sdo32(sc, 0x1600, 3, 0x60600008);// '-- Modes of operation
  ecrt_slave_config_sdo32(sc, 0x1600, 4, 0x27FE0008);//'-- Dummy Byte
  ecrt_slave_config_sdo32(sc, 0x1600, 5, 0x60B20010);//'-- Torque Offset
  ecrt_slave_config_sdo32(sc, 0x1600, 6, 0x60710010);// '-- Target Torque

  //Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1600, 0, 6);
  
  //------------------------------- 0x1610 -------------------------------
  //Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1600, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1610, 1, 0x60400010);// '-- Control Word
  ecrt_slave_config_sdo32(sc, 0x1610, 2, 0x607A0020);//'-- Target Position
  ecrt_slave_config_sdo32(sc, 0x1610, 3, 0x60600008);// '-- Modes of operation
  ecrt_slave_config_sdo32(sc, 0x1610, 4, 0x27FE0008);//'-- Dummy Byte
  ecrt_slave_config_sdo32(sc, 0x1610, 5, 0x60B20010);//'-- Torque Offset
  ecrt_slave_config_sdo32(sc, 0x1610, 6, 0x60710010);// '-- Target Torque

  //Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1610, 0, 6);
  // ******************************************************
  // Set Tx PDO Entries object
  ecrt_slave_config_sdo8(sc, 0x1C13, 0, 0);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1A00);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1A10);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1A01);
  ecrt_slave_config_sdo16(sc, 0x1C12, 1, 0x1A10);
  // Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1C13, 0, 4);
  
//---------------------------------- 0x1A00 -------------------------------------------------
  // Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1A00, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1A00, 1, 0x60410010);// '-- Status Word
  ecrt_slave_config_sdo32(sc, 0x1A00, 2, 0x60640020);// '-- Position Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A00, 3, 0x60770010);// '-- Torque Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A00, 4, 0x60610008);// '-- Mode of Operation Display
  ecrt_slave_config_sdo32(sc, 0x1A00, 5, 0x27FF0008);// '-- Dummy Byte

  //Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1A00, 0, 5);
  
 //---------------------------------- 0x1A10 -------------------------------------------------
  // Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1A10, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1A10, 1, 0x60410010);// '-- Status Word
  ecrt_slave_config_sdo32(sc, 0x1A10, 2, 0x60640020);// '-- Position Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A10, 3, 0x60770010);// '-- Torque Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A10, 4, 0x60610008);// '-- Mode of Operation Display
  ecrt_slave_config_sdo32(sc, 0x1A10, 5, 0x27FF0008);// '-- Dummy Byte

  //Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1A10, 0, 5);

  //-------------------------0x1A01 ------------------------------------------
  // Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1A01, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1A01, 1, 0x603F0010);// '-- Error Code
  ecrt_slave_config_sdo32(sc, 0x1A01, 2, 0x60F40020);// '-- Following Error Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A01, 3, 0x27040010);//  '-- warning code
  ecrt_slave_config_sdo32(sc, 0x1A01, 4, 0x60FD0020);//  '-- Digital IO status

  // Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1A01, 0, 4);
  //---------------------------------------------------------------------------
   //-------------------------0x1A11 ------------------------------------------
  // Zero number of entries
  ecrt_slave_config_sdo8(sc, 0x1A11, 0, 0);

  ecrt_slave_config_sdo32(sc, 0x1A11, 1, 0x603F0010);// '-- Error Code
  ecrt_slave_config_sdo32(sc, 0x1A11, 2, 0x60F40020);// '-- Following Error Actual Value
  ecrt_slave_config_sdo32(sc, 0x1A11, 3, 0x27040010);//  '-- warning code
  ecrt_slave_config_sdo32(sc, 0x1A11, 4, 0x60FD0020);//  '-- Digital IO status

  // Set number of entries
  ecrt_slave_config_sdo8(sc, 0x1A11, 0, 4);
  //---------------------------------------------------------------------------

}

 EL_Slave_t  slaves[16];

int  GetSlavesInfo(EL_Slave_t* slaves, int num)
{
   // ec_master_t* master = ecrt_request_master(0);
    ec_slave_info_t  tmps;
    int  i;
    for(i = 0;i< num;i++)
    {
        if(ecrt_master_get_slave(master, i, &tmps) == 0)
        {
            (slaves + i)->position = tmps.position;
            (slaves + i)->vendor_id = tmps.vendor_id;
            (slaves + i)->product_code = tmps.product_code;
            printf("slave position:%d,vendor_id:0x%x,product_code:0x%x,name:%s\n",
                   tmps.position, tmps.vendor_id, tmps.product_code, tmps.name);
        }
        else
            break;
    }
    //ecrt_release_master(master);
    return i;
}






